#include "Imagepro.hpp"

// 假设图像高度（行数）为 240，可根据实际图像尺寸调整
const int ROWS = 120;  // 图像行数（需与实际处理的图像高度一致）
float halfWidth[ROWS]; // 储存每行赛道宽度的一半（半宽值）

void initHalfWidth() {
    for (int i = 0; i < ROWS; ++i) {
        if (i < 10) {
            halfWidth[i] = 6.0f;
        } else if (i < 30) {
            halfWidth[i] = 8.0f;
        } else if (i < 50) {
            halfWidth[i] = 10.0f;
        } else if (i < 70) {
            halfWidth[i] = 12.0f;
        } else if (i < 90) {
            halfWidth[i] = 14.0f;
        } else if (i < 110) {
            halfWidth[i] = 16.0f;
        } else {
            halfWidth[i] = 18.0f;
        }
    }
}


Mat binary_img(Mat& img, unsigned short thre) {
    Mat view = applyPerspectiveTransform(img);
    if (view.empty())
        return Mat();
    // 图像预处理
    Mat gray, binary;
    cvtColor(view, gray, COLOR_BGR2GRAY);
    GaussianBlur(gray, gray, Size(7, 7), 3, 3);
    uint8_t threshold_ = otsuThreshold(gray);
    threshold(gray, binary, threshold_, 255, THRESH_BINARY);
    return binary;
}



Mat processImage(Mat& binary, float &angle, int& WC, int& chujie, int& zerbra, int& flag_left, int& flag_right) {
    zerbra = zebra(binary);
    WC = findWhiteColumn(binary);    // 长白列
    // int p = getLongestWhiteColumnIndex(binary);
    chujie = isParking(binary);

    int dx = 1;

    // 创建标记图像
    Mat colorResult;
    cvtColor(binary, colorResult, COLOR_GRAY2BGR);

    // 十字识别
    int rowsWithWhiteOver = 0;
    // 第一次遍历：统计满足条件的行数
    for (int y = 19; y <= 59; ++y) {
        int whiteCount = 0;
        // 此处也可改为指针访问，若需优化可补充
        for (int x = 0; x < binary.cols; ++x) {
            if (binary.at<uchar>(y, x) == 255) {
                whiteCount++;
            }
        }
        if (whiteCount > 110) {
            rowsWithWhiteOver++;
        }
    }
    bool shouldMark = (rowsWithWhiteOver >= 20);
    
    // 处理每一行，寻找左右边缘并追踪（核心：指针访问优化）
    vector<Point> centerPoints;
    int MID = -1;
    int preMID = -1;

    // 从图像底部往上处理每一行
    for (int y = binary.rows - 1; y >= 0; --y) {
        // 获取当前行的指针（关键修改：使用指针访问像素）
        uchar* row_ptr = binary.ptr<uchar>(y);  // 指向第y行首地址的指针
        
        int middleCol = binary.cols / 2;
        int startX = (MID != -1) ? MID : middleCol;

        // 查找左侧边缘点（从startX往左）
        int leftX = -1;
        for (int x = startX; x > 0; --x) {
            // 指针访问：row_ptr[x] 等价于 binary.at<uchar>(y, x)
            if (row_ptr[x] == 255 && row_ptr[x - 1] == 0) {
                leftX = x;
                break;
            }
        }

        // 查找右侧边缘点（从startX往右）
        int rightX = -1;
        for (int x = startX; x < binary.cols - 1; ++x) {
            // 指针访问：避免at方法的边界检查开销
            if (row_ptr[x] == 255 && row_ptr[x + 1] == 0) {
                rightX = x;
                break;
            }
        }

        // 标记边缘点
        Point leftPt(leftX, y);
        Point rightPt(rightX, y);
        // if (leftPt.x != -1) {
        //     circle(colorResult, leftPt, 0.6, RED, -1); // 红色标注左边沿
        // }
        // if (rightPt.x != -1) {
        //     circle(colorResult, rightPt, 0.6, BLUE, -1); // 蓝色标注右边沿
        // }

        // 历史边缘点缓存（用于突变检测）
        static int preleft[10] = {-1}; 
        static int preright[10] = {-1};
        // 滚动更新历史缓存
        for (int i = 9; i > 0; --i) {
            preleft[i] = preleft[i - 1];
            preright[i] = preright[i - 1];
        }
        preleft[0] = leftX;
        preright[0] = rightX;

        // 如果左右点都存在，则计算中点
        if (leftX != -1 && rightX != -1) {
            int rawCurrentCenterX = ((leftX + rightX) / 2 );
            Point rawCenter(rawCurrentCenterX, y);
            centerPoints.push_back(rawCenter);
            circle(colorResult, rawCenter, 0.6, GREEN, -1);
            MID = rawCurrentCenterX;
            if(abs(MID - preMID) > 6 )circle(colorResult, Point(MID, y), 2, RED, -1);
            preMID = MID;
        }
    }

    static float prevAngle = 0.0; 
    const float maxDelta = 18.0;  

    if (!centerPoints.empty()) {
        const int imageMidX = colorResult.cols / 2 ;
        int imageMidY_;

        // 动态调整中间Y坐标
        if(WC < 106){
            imageMidY_ = colorResult.rows * 0.23 ;
        }else if(WC < 112){
            imageMidY_ = colorResult.rows * 0.21;
        }else{
            imageMidY_ = colorResult.rows * 0.2;
        }
        // circle(colorResult, Point(imageMidX, imageMidY_), 2, RED, -1);

        // 权重参数
        float decayRateBelow = 0.15;
        float decayRateAbove = 0.16;
        const float baseWeight = 1.0;
        int imageMidY = imageMidY_ + 0.03;

        // 进十字修改权重
        if (shouldMark){
            imageMidY = imageMidY_ + 50;
            decayRateBelow = 0.02;
            decayRateAbove = 1;
        }

        float weightedSum = 0.0;
        float totalWeight = 0.0;

        // 绘制图像中线
        // line(colorResult, Point(imageMidX, 0), Point(imageMidX, colorResult.rows),
        //      Scalar(0, 255, 255), 1);

        // 计算加权角度
        for (const auto &pt : centerPoints) {
            int diff = pt.x - imageMidX;
            float weight;
            if (pt.y > imageMidY) {
                weight = baseWeight * exp(-decayRateBelow * (pt.y - imageMidY));
            } else {
                weight = baseWeight * exp(-decayRateAbove * (imageMidY - pt.y));
            }
            weightedSum += diff * weight;
            totalWeight += weight;
            float radius = weight * 5.0f;
            // circle(colorResult, pt, static_cast<int>(radius), Scalar(255, 0, 0), -1);
        }

        if (totalWeight > 0) {
            int avgDiff = (weightedSum / totalWeight);
            // 突变检测
            if (abs(avgDiff - prevAngle) > maxDelta) {
                angle = (prevAngle + avgDiff) * 0.5;
            } else {
                angle = avgDiff;
            }
            prevAngle = angle;
            // putText(colorResult, "angle: " + to_string(int(angle)),
            //         Point(40, 40), FONT_HERSHEY_SIMPLEX, 0.3, PURPLE, 2);
        }
    } else {
        angle = prevAngle;  // 保持前一次角度
    }

    // circle(colorResult, Point(0, 0), 2, RED, -1);

    // circle(colorResult, Point(160, 120), 2, RED, -1);

    return colorResult;
}